import rospy
from sensor_msgs.msg import Imu
from geometry_msgs.msg import Vector3, Quaternion


def imu_callback(imu_msg):
    # 获取IMU数据
    linear_acceleration = imu_msg.linear_acceleration
    angular_velocity = imu_msg.angular_velocity
    orientation = imu_msg.orientation
    # 打印IMU数据
    rospy.loginfo("Linear acceleration: x=%f, y=%f, z=%f", linear_acceleration.x, linear_acceleration.y,
                  linear_acceleration.z)
    rospy.loginfo("Angular velocity: x=%f, y=%f, z=%f", angular_velocity.x, angular_velocity.y, angular_velocity.z)
    rospy.loginfo("Orientation: x=%f, y=%f, z=%f, w=%f", orientation.x, orientation.y, orientation.z, orientation.w)


if __name__ == '__main__':
    # 初始化ROS节点
    rospy.init_node('imu_listener')
    # 订阅IMU数据
    imu_sub = rospy.Subscriber('imu/data', Imu, imu_callback)
    # 循环等待回调函数
    rospy.spin()
